#include <iostream>
#include "assist_avoid.h"

using namespace std;

#define SBUF_SIZE 512


hbAssistAvoid::hbAssistAvoid():
	m_djiFlightHandler(NULL),
	m_assistAvoidModeRunFlag(false),
	m_haveFlightControlAuthorityFlag(false)
{

}

hbAssistAvoid::~hbAssistAvoid()
{

}

void hbAssistAvoid::init(djiFlightHandler *handler)
{
	m_djiFlightHandler = handler;
}

void hbAssistAvoid::sleepMs(uint32_t timeMs)
{
	m_djiFlightHandler->sleepMs(timeMs);
}

void hbAssistAvoid::setAssistAvoidModeRunFlag(bool stats)
{
	m_assistAvoidModeRunFlag = stats;
}

bool hbAssistAvoid::getAssistAvoidModeRunFlag()
{
	return m_assistAvoidModeRunFlag;
}

bool hbAssistAvoid::obtainJoyStickCtrlAuthority()
{
	bool ret;

	if(m_haveFlightControlAuthorityFlag)
	{
		return true;
	}

	ret = m_djiFlightHandler->obtainJoyStickCtrlAuthority(AssistAvoidMode);

	if(!ret)
	{
		writeLogFile("hbAssistAvoid obtainJoyStickCtrlAuthority err\n");
		return false;
	}
	m_djiFlightHandler->setAssistAvoidSystemStatus(controlAovid);
	m_haveFlightControlAuthorityFlag = true;
	return true;
}


void hbAssistAvoid::releaseJoyStickCtrlAuthority()
{
	if(!m_haveFlightControlAuthorityFlag)
	{
		return;
	}

	m_haveFlightControlAuthorityFlag = false;
	m_djiFlightHandler->releaseJoyStickCtrlAuthority(AssistAvoidMode);
	m_djiFlightHandler->setAssistAvoidSystemStatus(moditorFlightState);
}

void hbAssistAvoid::writeLogFile(const char * format, ...)
{
	char sbuf[SBUF_SIZE];
	va_list args;
	va_start (args, format);
	vsnprintf (sbuf,SBUF_SIZE,format, args);
	va_end (args);

	m_djiFlightHandler->writeLogFile("%s", sbuf);
}

void hbAssistAvoid::run()
{
	if(NULL == m_djiFlightHandler)
	{
		return;
	}

	m_djiFlightHandler->setAssistAvoidSystemStatus(moditorFlightState);

	m_assistAvoidModeRunFlag = true;
	while(m_assistAvoidModeRunFlag)
	{
		T_DjiDataTimestamp messageTimestamp;
		m_djiFlightHandler->getFlightMode(m_flightCurrentMode, messageTimestamp);
		m_djiFlightHandler->getFlightStatus(m_flightCurrentStatus, messageTimestamp);

		switch(m_flightCurrentStatus)
		{
			case DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_STOPED:
				break;

			case DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_ON_GROUND:
				break;

			case DJI_FC_SUBSCRIPTION_FLIGHT_STATUS_IN_AIR:
				break;

			default:
					break;
		}

		//flight control mode change:41-->11-->14-->15-->12-->33->6
		switch(m_flightCurrentMode)
		{
			//mode 0: stablize mode
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_MANUAL_CTRL:
				break;
			
			//mode 1: althold mode
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_ATTITUDE:
				break;
			
			//mode 6: loiter or poshold mode, psdk control flight in this mode.
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_P_GPS:
				break;

			//mode 11: copter take off to 1 meter high
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_AUTO_TAKEOFF:
				break;

			//mode 12:copter land
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_AUTO_LANDING:
				break;

			//mode 14: auto execute the waypoint
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_RESERVED_14:
				break;

			//mode 15: rtl with the current height
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_NAVI_GO_HOME:
				break;

			//mode 17: controled by SDK API
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_NAVI_SDK_CTRL:
				break;

			//mode 33: force copter land
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_FORCE_AUTO_LANDING:
				break;

			//mode 41: motor start rotating, idling status  on the ground
			case DJI_FC_SUBSCRIPTION_DISPLAY_MODE_ENGINE_START:
				break;

			default:
            	break;
		}

		sleepMs(1000);
	}
}

hbAssistAvoid assistAvoid;
